% aniplot(data)
%
%  Description:
%   Generates an animated plot showing the motion of the system with
%   additional plots showing the phase portrait and angle and angle
%   velocities measured over time.
%
%  Operational Description:
%   Plots the angle/angle velocity and phase portrait graphs.  Creates
%   markers as plot objects that match up with animation graph, updating
%   every time the animation graph is updated.  The animation is a plot
%   object containing points for the end of each link.  This is updated for
%   every data point to generate an animation.
%
%  Arguments:
%   data    - A struct containing a an 'integral_curves' member.  This
%             member holds the results of integration returned by
%             hybrid_flow().
%
%  Return Values:
%   None.
%
function plot_tiles(data, filename)
addpath('../');
addpath('../build/');

% Parse input.
%
curves = cat(2, data.integral_curves{:});
t = cat(2, data.time{:});
q = curves;
dim = size(q, 1);

posv = (1:floor(dim/2));
vel = (floor(dim/2)+1:dim);

%% Setup the legends.
%
names = cell(1, dim);

for n=1:length(posv)
    names{posv(n)} = ['$q_' num2str(n) '$'];
    names{vel(n)} = ['${\dot q}_' num2str(n) '$'];
end

figpath = [
    '..' filesep ...
    '..' filesep ...
    'docs' filesep ...
    'report' filesep ...
    'figures' filesep];

linespecs = {'k.', 'k.', 'k.', 'k.', 'k.', 'k.', 'k.'};

%% Setup the plot for the animation.
%
f = figure(2598);
clf;
set(f, 'NumberTitle', 'off', ...
    'Name', 'MEEN 652: Flat Ground Walking Tiles', ...
    'Position', [560 528 600 170], ...
    'PaperPosition', [2 3 4 1.5]);

hold on;
axis([-.5 3.5 -.1 .69]);
axis manual;
origin = [0; 0];

grid on

xbnd = [-1 4];
dt = 1e-2;

fh = @get_terrain;

x = xbnd(1):dt:xbnd(2);
y_int = fh(0);
y = fh(x);

patch(...
    [-.5, x, 3.5], ... x
    [-.1, (y-y_int), -.1], ...
    ... %'Color', [108 52 28]/255);%, ... y
    0, ... color data
    'EdgeColor', 'None', ... no lines
    'FaceColor', [108 52 28]/255); % brown

set(gca, 'Position', [.05 0.05 .92 1])


k = 0;
tstep = .25;

%% Precompute plot positions.
%
xpos = cell(length(data.integral_curves), 1);
for i=1:length(data.integral_curves)
    %% Find the current positions of all the ends of the links, i.e., feet,
    %  knees, hip.
    %
    xpos{i} = jointpos(data.integral_curves{i});
end

for i=1:length(data.integral_curves)
    pos = xpos{i};
    %% Loop through all the data points in each discrete state.
    %
    for j = 1:size(pos, 3);
        % fix this for making videos by adding an option
        if (data.time{i}(j) < k*tstep)
            continue
        end
        
        k = k + 1;
        if k == 1
            axis equal
            ax = gca();
            % xl = get(ax, 'XLim');
            % yl = get(ax, 'YLim');
            
            set(ax, 'XLim', [-.5 3.25]);
            set(ax, 'YLim', [-0.1, .7]);
            
            % if opts.extragraphs
            %     set(gca, 'Position', [0.05 0.05 .90 .1]);
            % end
            % set(get(gca, 'XLabel'), ...
            %     'Position', [.51 -.3 1.00]);
        end
        
        % Update the positions of the legs and feet.
        %

        x_l = origin(1) + pos(1,  :, j);
        y_l = origin(2) + pos(2,  :, j);
        
        %% Replot the legs.
       
        plot(x_l, y_l);

    end
    
    % Update the position of the stance foot
    origin = origin + pos(:, end, end);
end
hold off;

hgexport(gcf, [figpath filename '.eps']);

end